
/****************************************************************
 * description     :    saliency.c 
 * author          :    Wang Mingyin
 * time            :    July 15th ~ August 9th, 2013
 ****************************************************************/
#include "saliency.h"
#include "graphcut.h"
#include "math.h"
#include <stdio.h>
#include <cstring>
#include "roc_statistic.h"
#include "color_quantization.h"

char g_output_filename[256];

float g_af_threshold = 0.0;
float g_roughmap_threshold = 0.0;

/****************************************************************************************
  *******************             file related data                   ********************
  ****************************************************************************************/
static BayesModel g_bayes_model[FEATURE_NUMBER]; //202 * 4 * 3 = 2424byte = 2.5kb
static float g_degree_backdiff[HISTOGRAM_SIZE + 1] = {0}; //read from file 65 * 4 = 260byte = 0.26kb
static float g_degree_shortpath[HISTOGRAM_SIZE + 1] = {0}; //0.26kb
static WeakLearner g_weak_learner[DICT_SIZE];                     // 16 * 500 = 8kb
static double g_code_book[CODE_SIZE][GRID_SIZE] = {0};  //512 * 16 * 8 = 65536 byte = 65kb
//median filter use 1.25kb
//SaliencyExist 1.51kb

//bayes probaqbility
static unsigned char g_cluster_map[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0}; //25 kb
static float g_xyz_data[THREE_CHANNEL_IMAGE_MAX_PIXEL] = {0};  //300 kb
static RGBCluster g_cluster_value[CLUSTER_SIZE]; // 4 * 3 * 16 = 192byte = 0.2kb
static double g_feature[CLUSTER_SIZE][GRID_SIZE] = {0.0}; //16 * 64 * 8 = 8192 byte = 8kb

//for shortpath feature
static unsigned char g_gray_img_data[THREE_CHANNEL_IMAGE_MAX_PIXEL] = {0}; //25 kb after median filter, for shotpath
static unsigned char g_new_img_data[THREE_CHANNEL_IMAGE_MAX_PIXEL] = {0}; //75 kb after median filter, for shotpath
static float g_shortpath_norm[MAX_LIMIT_LENGTH] = {0.0}; //768 * 4 = 3072byte = 3kb
static float g_feature_shortpath[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0}; //100 kb  //g_feature_shortpath => g_result_fore
static float g_result_back[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0}; //100 kb  //g_result_back
static float g_result_fore[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0}; //100 kb  //g_result_fore

//for backdiff feature
static float g_boundary_area[CLUSTER_SIZE] = {0.0};  //16 * 4 = 64 byte
static float g_center_area[CLUSTER_SIZE] = {0.0};  //64byte
static float g_ratio[CLUSTER_SIZE] = {0.0};  // 64 byte
static float g_boundary_diff[CLUSTER_SIZE] = {0.0}; //64 byte
static float g_backdiff_value[CLUSTER_SIZE] = {0.0}; //64byte
static struct FloatCount g_sort_array[CLUSTER_SIZE]; //16 * 4 * 3 = 192byte
static int g_cluster_count[CLUSTER_SIZE] = {0}; //every index count

//for yes or no feature
static float g_yon_value[CLUSTER_SIZE] = {0.0}; //64byte

void ShortpathFeature(unsigned char *img_data, float *result, int width, int height, int iter_num) {
	int img_width_step = CALCULATE_STEP_THREE(width);
	int res_width_step = width;
	ColorRGB aa, bb, cc, dd, ee, ff, gg, hh, ii;
	int length = width * height;
	float delta_a, delta_b, delta_c, delta_d, delta_i, delta_h, delta_g, delta_f;
	int xx, yy;
	int xx_0, xx_left, xx_right;
	int y, x;
	int k;
	//init the value
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			IMAGE_ELEM(result, float, res_width_step, x, y) = FLOAT_MAX;
		}
	}

	for (k = 0; k < iter_num; k++) {
		for (y = 0; y < height; y++) {
			for (x = 0; x < width; x++) {
				xx = x;
				yy = y;
				xx_0 = xx + xx + xx;
				xx_left = xx_0 - 3;
				xx_right = xx_0 + 3;
				ee.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 0, yy);
				ee.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 1, yy);
				ee.b = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 2, yy);

				xx = x - 1;
				yy = y - 1;
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_a = 0;
				} else	{
					aa.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 0, yy);
					aa.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 1, yy);
					aa.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 2, yy);
					delta_a = ColorSquDistance(ee, aa) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}

				xx = x;
				yy = y - 1;//bb
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_b = 0;
				} else	{
					bb.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 0, yy);
					bb.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 1, yy);
					bb.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 2, yy);
					delta_b = ColorSquDistance(ee, bb) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}				

				xx = x +1;
				yy = y - 1;//cc
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_c = 0;
				} else {
					cc.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 0, yy);
					cc.g = IMAGE_ELEM(img_data, unsigned char, img_width_step,xx_right + 1, yy);
					cc.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 2, yy);
					delta_c = ColorSquDistance(ee, cc) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}			

				xx = x - 1;
				yy = y ;//dd
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_d= 0;
				} else {
					dd.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 0, yy);
					dd.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 1, yy);
					dd.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 2, yy);
					delta_d= ColorSquDistance(ee, dd) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}	

				IMAGE_ELEM(result, float, res_width_step, x, y) = MIN(IMAGE_ELEM(result, float, res_width_step, x, y), MIN(MIN(delta_a, delta_b), MIN(delta_c, delta_d)));
			}
		}

		for (y = height - 1; y >= 0; y--) {
			for (x = width - 1; x >= 0; x--) {
				xx = x;
				yy = y; //ee
				xx_0 = xx + xx + xx;
				xx_left = xx_0 - 3;
				xx_right = xx_0 + 3;

				ee.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 0, yy);
				ee.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 1, yy);
				ee.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 2, yy);

				xx = x + 1;
				yy = y + 1; //ii
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_i = 0;
				} else {
					ii.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 0, yy);
					ii.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 1, yy);
					ii.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 2, yy);
					delta_i = ColorSquDistance(ee, ii) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}
		
				xx = x;
				yy = y + 1;//hh
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_h = 0;
				} else {
					hh.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 0, yy);
					hh.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 1, yy);
					hh.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_0 + 2, yy);
					delta_h = ColorSquDistance(ee, hh) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}
				
				xx = x - 1;
				yy = y + 1;//gg
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_g = 0;
				} else {
					gg.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 0, yy);
					gg.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 1, yy);
					gg.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_left + 2, yy);
					delta_g = ColorSquDistance(ee, gg) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}

				xx = x + 1;
				yy = y;//ff
				if (xx < 0 || yy < 0 || xx >= width || yy >= height) {
					delta_f = 0;
				} else {
					ff.r = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 0, yy);
					ff.g = IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 1, yy);
					ff.b= IMAGE_ELEM(img_data, unsigned char, img_width_step, xx_right + 2, yy);
					delta_f = ColorSquDistance(ee, ff) + 1 + IMAGE_ELEM(result, float, res_width_step, xx, yy);
				}

				IMAGE_ELEM(result, float, res_width_step, x, y) = MIN(IMAGE_ELEM(result, float, res_width_step, x, y), MIN(MIN(delta_h, delta_i), MIN(delta_g, delta_f)));
			}
		}
	}//end outer  for (int k = 0; i < iter_num; k++) 

	ShortPathNormalize(result, length, NORMALIZE_LIMIT_RATE);
}



void BackDiffFeatureNew(unsigned char *clustermap, int width, int height, RGBCluster *cluster_values, int margin, float *backdiff_value) {
	float sigma = 20;
	float exp_sig = (-0.5f) / (sigma * sigma);
	int cluster_width_step = width; //single channel
	
	
	int x, y;
	int i, j;
	float x1, y1, z1, x2, y2, z2;
	float dist;
	int length = width * height;

	memset(g_boundary_area, 0, sizeof(g_boundary_area));
	memset(g_center_area, 0, sizeof(g_center_area));
	memset(g_center_area, 0, sizeof(g_center_area));
	memset(g_ratio, 0, sizeof(g_ratio));
	memset(g_sort_array, 0, sizeof(g_sort_array));
    memset(g_cluster_count, 0, sizeof(g_cluster_count));

	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			int index = IMAGE_ELEM(clustermap, unsigned char, cluster_width_step, x, y);
			g_cluster_count[index]++;
			if (x < margin || y < margin || x >= width - margin || y >= height - margin) {
				g_boundary_area[index]++;
			} else {
				g_center_area[index]++;
			}
		}
	}
	
	for (i = 0; i < CLUSTER_SIZE; i++) {
		x1 = cluster_values[i].r;
		y1 = cluster_values[i].g;
		z1 = cluster_values[i].b;
		g_boundary_diff[i] = 0;
		for (j = 0; j < CLUSTER_SIZE; j++) {
			if (g_boundary_area[j] > 0) {
				x2 = cluster_values[j].r;
                y2 = cluster_values[j].g;
                z2 = cluster_values[j].b;

                dist = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2);
                dist = (float)exp(exp_sig * dist); //exp(-0.5f / (sigma * sigma) * dist2);
				g_boundary_diff[i] += dist * g_boundary_area[j];
			}//end if
		} // end for(int j = 0;...)
		g_ratio[i] = 1 - g_center_area[i] / (g_center_area[i] + g_boundary_area[i] + 0.00000001f);
	}

	//init the struct array
	for (i = 0; i < CLUSTER_SIZE; i++) {
		g_sort_array[i].value = g_boundary_diff[i];
		g_sort_array[i].count = g_cluster_count[i];
		g_sort_array[i].index = i;
	}
	
	BackDiffNormalizeNew(g_sort_array, length, NORMALIZE_LIMIT_RATE);
	//recover the index count
	for (i = 0; i < CLUSTER_SIZE; i++) {
		g_sort_array[i].count = g_cluster_count[g_sort_array[i].index];
	}
	//compare with the g_ratio index
	for (i = 0; i < CLUSTER_SIZE; i++) {
		g_sort_array[i].value = MAX(g_sort_array[i].value, g_ratio[g_sort_array[i].index]);
	}

	BackDiffNormalizeNew(g_sort_array, length, NORMALIZE_LIMIT_RATE);

	//store the nomalized value in the array sorted by index
	for (i = 0; i < CLUSTER_SIZE; i++) {
		for (j = 0; j < CLUSTER_SIZE; j++) {
			if (g_sort_array[j].index == i) {
				break;
			}
		}
		backdiff_value[i] = g_sort_array[j].value;
	}
}

void BackDiffNormalizeNew(struct FloatCount *parray, int length, float rate) {
	int limit = (int)(length * rate + 0.5f);
	int min_index = limit;
    int max_index = length - limit;
	float max_v, min_v;
	float diff;
	int i, j;

	//sort the struct array(bubble sort by value)
	for (i = 0; i < CLUSTER_SIZE - 1; i++) {
		for (j = i + 1; j < CLUSTER_SIZE; j++) {
			if (parray[i].value > parray[j].value) {
				struct FloatCount tmp = parray[i];
				parray[i] = parray[j];
				parray[j] = tmp;
			}
		}
	}

	for (i = 1; i < CLUSTER_SIZE; i++) {
		parray[i].count += parray[i - 1].count;
	}
	for (i = 0; i < CLUSTER_SIZE; i++) {
		if (min_index < parray[i].count) {
			min_v = parray[i].value;
			break;
		}
	}
	for (i = CLUSTER_SIZE - 1; i > 0; i--) {
		if (max_index < parray[i].count && max_index >= parray[i - 1].count) {
			max_v = parray[i].value;
			break;
		}
	}
	diff = max_v - min_v;
	
	for (i = 0; i < CLUSTER_SIZE; i++) {
		if (parray[i].value < min_v) {
			parray[i].value = 0;
		} else if (parray[i].value > max_v) {
			parray[i].value = 1;
		} else {
			parray[i].value = (parray[i].value - min_v) / diff;
		}
	}

}
void ShortPathNormalize(float *parray, int length, float rate) {
	int limit = (int)(length * rate + 0.5f);
	float min_v, max_v;
	float diff;
	int i;

	limit++; //change to min heap, add one element!!!
	for (i = 0; i < limit; i++) {
		g_shortpath_norm[i] = parray[i];
	}
	MakeHeap(g_shortpath_norm, limit, MaxHeapCom);

	for (i = limit; i < length; i++) {
		if (parray[i] < g_shortpath_norm[0]) { //change element, and adjust
			g_shortpath_norm[0] = parray[i];
			AdjustHeap(g_shortpath_norm, 0, limit, MaxHeapCom);
		}
	}
	min_v = FindMaxOrMin(g_shortpath_norm, limit, MinHeapCom); //find the max value

	limit--; //change to min heap, back to normal!!!
	for (i = 0; i < limit; i++) {
		g_shortpath_norm[i] = parray[i];
	}
	MakeHeap(g_shortpath_norm, limit, MinHeapCom);

	for (i = limit; i < length; i++) {
		if (parray[i] > g_shortpath_norm[0]) { //change element, and adjust
			g_shortpath_norm[0] = parray[i];
			AdjustHeap(g_shortpath_norm, 0, limit, MinHeapCom);
		}
	}
	max_v = FindMaxOrMin(g_shortpath_norm, limit, MaxHeapCom); //find the min value
	
	diff = max_v - min_v;
	
	for (i = 0; i < length; i++) {
		if (parray[i] < min_v) {
			parray[i] = 0;
		} else if (parray[i] > max_v) {
			parray[i] = 1;
		} else {
			parray[i] = (parray[i] - min_v) / diff;
		}
	}
}


float Classify(WeakLearner *weak_learner, int weak_length, float *sample, int sample_length) {
	float sum = 0;
	float f = 0.0f, hf = 0.0f;
	int i;
	for (i = 0; i < weak_length; i++) {
		f = sample[weak_learner[i].index];
		if (f < weak_learner[i].threshold) {
			hf = weak_learner[i].conf1;
		} else {
			hf = weak_learner[i].conf2;
		}
		sum += hf;
	}
	return sum;
}

int GetClassifyResult(double feature[][GRID_SIZE], double code_book[][GRID_SIZE], unsigned char *out_data, int width, int height) {
	double one_feature[GRID_SIZE] = {0.0};
	float code_result[CODE_SIZE] = {0.0};
	int res_width_step = width;
	
	float score;
	int i, k;
	int x, y;
	int code_length = CODE_SIZE;
	int saliency_points = 0;

    memset(g_cluster_count, 0, sizeof(g_cluster_count));

	for (i = 0; i < CLUSTER_SIZE; i++) {
		if (feature[i][0] - (-1) < EPSILON && (-1) - feature[i][0] < EPSILON) {
			continue;
		}
		for (k = 0; k < GRID_SIZE; k++) {
			one_feature[k] = feature[i][k];
		}
		
		Encoder(one_feature, code_result, code_book, code_length);
		score = Classify(g_weak_learner, DICT_SIZE, code_result, GRID_SIZE);
		g_yon_value[i] = score;
	}
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			int index = IMAGE_ELEM(out_data, unsigned char, res_width_step, x, y);
			g_cluster_count[index]++;
		}
	}
	for (i = 0; i < CLUSTER_SIZE; i++) {
		if (g_yon_value[i] > 0) {
			saliency_points += g_cluster_count[i];
		}
	}
	
	return saliency_points;
}
void BayesClassifySimpleNew(BayesModel *bayes, int length, float *feature_shortpath, float *degree_shortpath, unsigned char *cluster_map, float *feature_backdiff, float *degree_backdiff,
						 float *feature_yon, float *result_back, int width, int height) {
	int width_step = width;
	float prob_fore = bayes[0].fore_prob, prob_back = bayes[0].back_prob;
	float fore, back;
	int x, y, i;
	int hist_index, index;
	float value;
	int hist_length = HISTOGRAM_SIZE;
	int feature_index;
	//for yon feature
	int bin_cnt = YOU_FEATURE_BIN_COUNT;
	float min_limit = -10.0f;
	float bin_gap = 0.2f;
	unsigned char backdiff_index;
	int yon_index;
    float tmp_prob_fore, tmp_prob_back;


	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			fore = prob_fore, back = prob_back;
			for (feature_index = 0; feature_index < length; feature_index++) {
				switch (feature_index) {
					case SHORTPATH_FEATURE_INDEX:
						value = IMAGE_ELEM(feature_shortpath, float, width, x, y);
						i = 0, index = 0;
						for (i = 0; i < hist_length ; i++) {
							if (value >= degree_shortpath[i] && value < degree_shortpath[i + 1]) {
								index = i;
								break;
							}
						}
						if (value >= BAYES_THRESHOLD)
							index = hist_length - 1;
						hist_index = index;
						break;
					case BACKDIFF_FEATURE_INDEX:
						backdiff_index = IMAGE_ELEM(cluster_map, unsigned char, width, x, y);
						value = feature_backdiff[backdiff_index];
						i = 0, index = 0;
						for (i = 0; i < hist_length ; i++) {
							if (value >= degree_backdiff[i] && value < degree_backdiff[i + 1]) {
								index = i;
								break;
							}
						}
						if (value >= BAYES_THRESHOLD)
							index = hist_length - 1;
						hist_index = index;
						break;
					case YON_FEATURE_INDEX: 
						//value = IMAGE_ELEM(feature_yon, float, width, x, y);
						yon_index = IMAGE_ELEM(cluster_map, unsigned char, width, x, y);
						value = feature_yon[yon_index];
						index = (int)((value - min_limit) / bin_gap);
						index = index < 0 ? 0 : index;
						index = index > bin_cnt - 1 ? bin_cnt - 1 : index;
						hist_index = index;
						break;
					default:
						printf("Error index\n");
				}
                tmp_prob_fore = bayes[feature_index].fore_hist[hist_index];
                tmp_prob_back = bayes[feature_index].back_hist[hist_index];

                if (feature_index == SHORTPATH_FEATURE_INDEX && SHORTPATH_ROC_MASK) {
                    tmp_prob_fore = 1;
                    tmp_prob_back = 1;
                }
                if (feature_index == BACKDIFF_FEATURE_INDEX && BACKDIFF_ROC_MASK){
                    tmp_prob_fore = 1;
                    tmp_prob_back = 1;
                }
                if (feature_index == YON_FEATURE_INDEX && YON_ROC_MASK){
                    tmp_prob_fore = 1;
                    tmp_prob_back = 1;
                }

                fore *= tmp_prob_fore;
                back *= tmp_prob_back;
			}
			IMAGE_ELEM(feature_shortpath, float, width_step, x, y) = fore;
			IMAGE_ELEM(result_back, float, width_step, x, y) = back;
		}
	}
}

void Encoder(double *feature, float *code_result, double code_book[][GRID_SIZE], int length) {
	double mean_dist = 0;
	int i, j;
	double sum;
	double normal = 0;

	for (i = 0; i < length; i++) {
		sum = 0;
		for (j = 0; j < GRID_SIZE; j++) {
			sum += (code_book[i][j] - feature[j]) * (code_book[i][j]- feature[j]);
		}
		code_result[i] = (float)(sqrt(sum));
		mean_dist += code_result[i];
	}
	mean_dist /= length; //average distance
	
	for (i = 0; i < length; i++) {
		code_result[i] = (float)(MAX(0, mean_dist - code_result[i])); //keep the value which is small than mean_dist
		normal += code_result[i];
	}
	for (i = 0; i < length; i++) {
		code_result[i] /= (float)normal;
	}
}


//saliency exist
//take care of the stack size, now is 10 * 1024 * 1024 byte, if all stack overflow, just use the malloc/free
int SaliencyExist(unsigned char *img_data, unsigned char *label_img_data, int width, int height) {
	int res_width_step = width;
	
	double color_count[CLUSTER_SIZE] = {0.0}; //16 * 8 = 128byte = 0.1kb
	double one_feature[GRID_SIZE] = {0.0};      //64 * 8 = 512byte = 0.5kb
	
	SegPos seg_pos[GRID_SIZE];                             //64 * 16 = 1024byte = 1kb
	
	unsigned char cluster_index = 0;
	int i, j, x, y, end_x, end_y;
	int saliency_points = 0;

	
	memset(seg_pos, 0, sizeof(seg_pos));
	memset(g_feature, 0, sizeof(g_feature));
	

	

	ImageSegmentation(width, height, HORIZON_BLOCK, VERTICAL_BLOCK, seg_pos);

	for (i = 0; i < GRID_SIZE; i++) {
		end_y = seg_pos[i].y + seg_pos[i].height;
		end_x = seg_pos[i].x + seg_pos[i].width;
		for (y = seg_pos[i].y; y < end_y; y++) {
			for (x = seg_pos[i].x; x < end_x; x++) {
				cluster_index = IMAGE_ELEM(g_cluster_map, unsigned char, res_width_step, x, y);
				g_feature[cluster_index][i]++;
				color_count[cluster_index]++;
			}
		}
	}

	for (i = 0; i < GRID_SIZE; i++) {
		for (j = 0; j < CLUSTER_SIZE; j++) {
			if (color_count[j] != 0) {
				g_feature[j][i] = g_feature[j][i] / color_count[j];
			} else {
				g_feature[j][i] = -1;
			}
		}
	}
	saliency_points = GetClassifyResult(g_feature, g_code_book, g_cluster_map, width, height);

    if (ROC_RATE_CHOICE == 0) {// 0 mean adaboost
	    YonRocRate(g_yon_value, g_cluster_map, width, height, label_img_data);
        SaliencyCountRocRate(g_yon_value, g_cluster_count, label_img_data, width, height);
    }
    if (WRITE_ALL_FLOAT_IMAGE == 1) {
        SaveYonImage(g_yon_value, g_cluster_map, width, height); //write the yon float image
    }

	return saliency_points;
}

void ImageSegmentation(int width, int height, int hor_block, int ver_block, SegPos *seg_pos) {
	int ave_height = height / hor_block;
	int ave_width = width / ver_block;
	SegPos sp;
	int i, j;
	int index = 0;
	for (i = 0; i < hor_block; i++)	{
		for (j = 0; j < ver_block; j++) {
			sp.x = j * ave_width;
			sp.y = i * ave_height;
			sp.width = ave_width;
			sp.height = ave_height;
			seg_pos[index++] = sp;
		}
	}
}


int SaliencyExistConfirm(unsigned char * AF_data, float *img_yon, int width, int height) {
	int x,y;
	unsigned char AF_pixel;
	float img_yon_pixel;
	int res_width_step =width;
	int image_width_step =width *3 ;
	float AF_count =0;
	float yon_count =0;
	float total_count = 0;
	int saliency_exist = 0;
	int Th_area =500;
	float Th_rate = 0.6;
	float rate_yon, rate_af,rate;

	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			AF_pixel = IMAGE_ELEM(AF_data, unsigned char, image_width_step, x * 3 , y);
			img_yon_pixel = IMAGE_ELEM(img_yon, float, res_width_step, x, y);

			if (AF_pixel != 0) {
				AF_count ++;
			}
			if (img_yon_pixel != 0)	{
				yon_count ++;
			}
			if (AF_pixel != 0 && img_yon_pixel != 0) {
				total_count++;
			}
		}
	}

	if (yon_count > Th_area) {
		rate_yon = total_count / yon_count;
		rate_af = total_count / AF_count;
		rate = MAX(rate_yon,rate_af);

		if (rate > Th_rate) {
			saliency_exist = 1;
		}
	}
	return saliency_exist;
}


int SaliencyDetectionForRoc(unsigned char *img_data, unsigned char *label_img_data, int width, int height, unsigned char *graphcut, unsigned char *rough_data, unsigned char *AF_data) {
	int margin = MARGIN;
	int iter_num = ITERATE_NUMBER;
    int saliency_points = 0;
    int saliency_have_th = SALIENCY_HAVE_THRESHOLD;
    int saliency_label = 0;
    float img_float[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0};


    // color quantization...
    memset(g_xyz_data, 0, sizeof(g_xyz_data));
	memset(g_cluster_value, 0, sizeof(g_cluster_value));
	memset(g_cluster_map, 0, sizeof(g_cluster_map));
 //   GetXYZClusterInfo(img_data, width, height, g_xyz_data, g_cluster_map, g_cluster_value); //resize
    GetXYZClusterInfoNew(img_data, width, height, g_xyz_data, g_cluster_map, g_cluster_value);

    // shortpath and backdiff
	memset(g_new_img_data,  0, sizeof(g_new_img_data));
	memset(g_gray_img_data, 0, sizeof(g_gray_img_data));

	memset(g_feature_shortpath, 0, sizeof(g_feature_shortpath));
	ShortpathFeature(img_data, g_feature_shortpath, width, height, iter_num);
	//ShortpathFeatureNew(g_new_img_data, g_feature_shortpath, width, height, iter_num);

	//2 backdiff feature
	memset(g_backdiff_value, 0, sizeof(g_backdiff_value));
	BackDiffFeatureNew(g_cluster_map, width, height, g_cluster_value, margin, g_backdiff_value);

	//write all the data
    if (WRITE_ALL_FLOAT_IMAGE == 1) {
        SaveShortpathImage(g_feature_shortpath, width, height);
        SaveBackdiffImage(g_backdiff_value, g_cluster_map, width, height);
    }
	
    
    //AFMap
    memset(g_result_back, 0, sizeof(g_result_back));
	memset(g_result_fore, 0, sizeof(g_result_fore));
	GetAFMap(g_bayes_model, FEATURE_NUMBER, g_feature_shortpath, g_degree_shortpath, g_cluster_map, g_backdiff_value, g_degree_backdiff, g_result_back, g_result_fore, width, height);
	ConvertAFMap(g_result_fore, g_result_back, AF_data, width, height);

    SaliencyExist(img_data, label_img_data, width, height);
    //if (saliency_points <= saliency_have_th) { 
    //    // saliency points are too little, no need to process
    //    return;
    //}

 //   
    memset(img_float, 0, sizeof(img_float));
    GetImageFloat(g_yon_value, g_cluster_map, img_float, width, height);
    saliency_label = SaliencyExistConfirm(AF_data, img_float,  width,  height);
    if (saliency_label == 0) {
        return 0;
    }

    
	memset(g_result_back, 0, sizeof(g_result_back));
	BayesClassifySimpleNew(g_bayes_model, FEATURE_NUMBER, g_feature_shortpath, g_degree_shortpath, g_cluster_map, g_backdiff_value, g_degree_backdiff, g_yon_value, g_result_back, width, height);

	
	GetRoughBmpOld(g_feature_shortpath, g_result_back, rough_data, width, height);

    
    
	//5 graphcut
	//GraphCutSegmentOld(img_data, width, height, g_feature_shortpath, g_result_back, graphcut);
	
    return saliency_label;
}
void GetAFMap(BayesModel *bayes, int length, float *feature_shortpath, float *degree_shortpath, unsigned char *cluster_map, float *feature_backdiff, float *degree_backdiff,
						 float *result_back, float *result_fore,int width, int height) {
	int width_step = width;
	float prob_fore = bayes[0].fore_prob, prob_back = bayes[0].back_prob;
	float fore, back;
	int x, y, i;
	int hist_index, index;
	float value;
	int hist_length = HISTOGRAM_SIZE;
	int feature_index;
	//for yon feature
	int bin_cnt = YOU_FEATURE_BIN_COUNT;
	float min_limit = -10.0f;
	float bin_gap = 0.2f;
	unsigned char backdiff_index;
	int yon_index;
    float tmp_prob_fore, tmp_prob_back;


	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			fore = prob_fore, back = prob_back;
			for (feature_index = 0; feature_index < 2; feature_index++) {
				switch (feature_index) {
					case SHORTPATH_FEATURE_INDEX:
						value = IMAGE_ELEM(feature_shortpath, float, width, x, y);
						i = 0, index = 0;
						for (i = 0; i < hist_length ; i++) {
							if (value >= degree_shortpath[i] && value < degree_shortpath[i + 1]) {
								index = i;
								break;
							}
						}
						if (value >= BAYES_THRESHOLD)
							index = hist_length - 1;
						hist_index = index;
						break;
					case BACKDIFF_FEATURE_INDEX:
						backdiff_index = IMAGE_ELEM(cluster_map, unsigned char, width, x, y);
						value = feature_backdiff[backdiff_index];
						i = 0, index = 0;
						for (i = 0; i < hist_length ; i++) {
							if (value >= degree_backdiff[i] && value < degree_backdiff[i + 1]) {
								index = i;
								break;
							}
						}
						if (value >= BAYES_THRESHOLD)
							index = hist_length - 1;
						hist_index = index;
						break;
					default:
						printf("Error index\n");
				}
                tmp_prob_fore = bayes[feature_index].fore_hist[hist_index];
                tmp_prob_back = bayes[feature_index].back_hist[hist_index];

                if (feature_index == SHORTPATH_FEATURE_INDEX && SHORTPATH_ROC_MASK) {
                    tmp_prob_fore = 1;
                    tmp_prob_back = 1;
                }
                if (feature_index == BACKDIFF_FEATURE_INDEX && BACKDIFF_ROC_MASK){
                    tmp_prob_fore = 1;
                    tmp_prob_back = 1;
                }

                fore *= tmp_prob_fore;
                back *= tmp_prob_back;
			}
			IMAGE_ELEM(result_fore, float, width_step, x, y) = fore;
			IMAGE_ELEM(result_back, float, width_step, x, y) = back;
		}
	}
}

void ConvertAFMap(float *result_fore, float *result_back, unsigned char *rough_data, int width, int height) {
	int x, y;
	float rate;
	int img_width_step = CALCULATE_STEP_THREE(width);
	int res_width_step = width;
	float max_f = -1000, min_f = 1000;
	float result_tmp[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0};
	int length = height * width;

	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			//rate = IMAGE_ELEM(result_fore, float, res_width_step, x, y);
			rate = IMAGE_ELEM(result_fore, float, res_width_step, x, y) / (IMAGE_ELEM(result_fore, float, res_width_step, x, y) + IMAGE_ELEM(result_back, float, res_width_step, x, y));
			float fore_prob = IMAGE_ELEM(result_fore, float, res_width_step, x, y);
            float back_prob = IMAGE_ELEM(result_back, float, res_width_step, x, y);

            rate = log10(fore_prob) - log10(back_prob);
            rate = rate >= g_af_threshold ? 255 : 0;
			//rate = rate > g_af_threshold ? IMAGE_ELEM(result_fore, float, res_width_step, x, y) : 0;
			IMAGE_ELEM(result_tmp, float, res_width_step, x, y) = rate;
			//if (max_f < rate) {
			//	max_f = rate;
			//}
			//if (min_f > rate) {
			//	min_f = rate;
			//}
		}
	}
	//ShortPathNormalize(result_tmp, length, NORMALIZE_LIMIT_RATE);
    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {

            rate = IMAGE_ELEM(result_tmp, float, res_width_step, x, y);
            if (max_f < rate) {
                max_f = rate;
            }
            if (min_f > rate) {
                min_f = rate;
            }
        }
    }
	//float to unsigned char data
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			//int val = (int)((IMAGE_ELEM(result_tmp, float, res_width_step, x, y) - min_f) * 255 / (max_f - min_f) + 0.5);
			int val = (int)(IMAGE_ELEM(result_tmp, float, res_width_step, x, y));
			if (val < 0) {
				val = 0;
			}
			if (val > 255) {
				val = 255;
			}
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 0, y) = val;
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 1, y) = val;
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 2, y) = val;
		}
	}
}





void GraphCutSegmentOld(unsigned char *img_data, int width, int height, float *fore_img, float *back_img, unsigned char *graphcut) {
	int node_num = width * height;
	int edge_num = (width - 1) * height + (height - 1) * width;
	
	int top = 0, bottom = height, left = 0, right = width;
	int index = 0;
	float weight_source, weight_sink;
	int width_step = width;
	int img_width_step = CALCULATE_STEP_THREE(width);
	float edge_weight_h, edge_weight_v;
	int x, y;

	create_graph(node_num, edge_num);

	for (y = top; y < bottom; y++) {
		for (x = left; x < right; x++) {
			graph_add_node();
		}
	}

	for (y = top; y < bottom; y++) {
		for (x = left; x < right; x++) {
			weight_source = -(float)log10(IMAGE_ELEM(back_img, float, width_step, x, y));
			weight_sink = -(float)log10(IMAGE_ELEM(fore_img, float, width_step, x, y));
			
			graph_add_tweights(index, weight_source, weight_sink);

			if (x < right - 1) {
				edge_weight_h = Boundary(img_data, width, height, x, y, x + 1, y);
				graph_add_edge(index, index + 1, edge_weight_h, edge_weight_h);
			}

			if (y < bottom - 1)	{
				edge_weight_v = Boundary(img_data, width, height, x, y, x, y + 1);
				graph_add_edge(index, index + width, edge_weight_v, edge_weight_v);
			}
			index++;
		}
	}
	graph_max_flow();

	index = 0;
	for (y = top; y < bottom; y++) {
		for (x = left; x < right; x++) {
			if (graph_get_segment(index) == 0) {
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 0, y) = 255;
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 1, y) = 255;
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 2, y) = 255;
			} else	{
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 0, y) = 0;
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 1, y) = 0;
				IMAGE_ELEM(graphcut, unsigned char, img_width_step, x * 3 + 2, y) = 0;
			}
			index++;
		}
	}
}
void GetRoughBmpOld(float *result_fore, float *result_back, unsigned char *rough_data, int width, int height) {
	int x, y;
	float rate;
	int img_width_step = CALCULATE_STEP_THREE(width);
	int res_width_step = width;
	float max_f = FLOAT_MIN, min_f = FLOAT_MAX;
	float result_tmp[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0};

	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			//rate = IMAGE_ELEM(result_fore, float, res_width_step, x, y);
            float a = IMAGE_ELEM(result_fore, float, res_width_step, x, y);
            float b = IMAGE_ELEM(result_back, float, res_width_step, x, y);
            rate = log10(a) - log10(b);
            rate = rate >= g_roughmap_threshold ? 255 : 0;

            //rate = a / (a + b);
			//rate = (float)IMAGE_ELEM(result_fore, float, res_width_step, x, y) / (float)(IMAGE_ELEM(result_fore, float, res_width_step, x, y) + IMAGE_ELEM(result_back, float, res_width_step, x, y));
			//rate = rate > g_roughmap_threshold ? rate : 0;
			IMAGE_ELEM(result_tmp, float, res_width_step, x, y) = rate;
			if (max_f < rate) {
				max_f = rate;
			}
			if (min_f > rate) {
				min_f = rate;
			}
		}
	}

	//float to unsigned char data
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			//int val = (int)((IMAGE_ELEM(result_tmp, float, res_width_step, x, y) - min_f) * 255 / (max_f - min_f) + 0.5);
			int val = (int)(IMAGE_ELEM(result_tmp, float, res_width_step, x, y));
            if (val < 0) {
				val = 0;
			}
			if (val > 255) {
				val = 255;
			}
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 0, y) = val;
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 1, y) = val;
			IMAGE_ELEM(rough_data, unsigned char, img_width_step, x * 3 + 2, y) = val;
		}
	}
}

float Boundary(unsigned char *img_data, int width, int height, int x1, int y1, int x2, int y2) {
	int img_width_step = CALCULATE_STEP_THREE(width);

	int a1 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x1 * 3 + 0, y1);
	int b1 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x1 * 3 + 1, y1);
	int c1 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x1 * 3 + 2, y1);
	int a2 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x2 * 3 + 0, y2);
	int b2 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x2 * 3 + 1, y2);
	int c2 = IMAGE_ELEM(img_data, unsigned char, img_width_step, x2 * 3 + 2, y2);

	int diff_a = fabs((float)(a1 - a2));
	int diff_b = fabs((float)(b1 - b2));
	int diff_c = fabs((float)(c1 - c2));

	float dist2 = (float)(diff_a * diff_a + diff_b * diff_b + diff_c * diff_c);
	return (float)(10) / (dist2 / (float)(500) + (float)(1));
}


void InitGlobalDataFromFile() {
	memset(g_code_book, 0, sizeof(g_code_book));
	ReadCodebook(CODE_BOOK_PATH, g_code_book); 

	memset(g_degree_backdiff, 0, sizeof(g_degree_backdiff));
	memset(g_degree_shortpath, 0, sizeof(g_degree_shortpath));
	ReadHistDegree(BACKDIFF_DEGREE_PATH, g_degree_backdiff); 
	ReadHistDegree(SHORTPATH_DEGREE_PATH, g_degree_shortpath);


	memset(g_bayes_model, 0, sizeof(g_bayes_model));
	ReadBayesModel(SHORTPATH_BAYES_PATH, &g_bayes_model[SHORTPATH_FEATURE_INDEX]);
	ReadBayesModel(BACKDIFF_BAYES_PATH, &g_bayes_model[BACKDIFF_FEATURE_INDEX]);
	ReadBayesModel(YON_BAYES_PATH, &g_bayes_model[YON_FEATURE_INDEX]);

	memset(g_weak_learner, 0, sizeof(g_weak_learner));
	ReadDictionary(BOOST_DICTIONARY_PATH, g_weak_learner);
}

/****************************************************************************************
  *******************                   read                          ********************
  ****************************************************************************************/
void ReadCodebook(const char *filename, double code_book[][GRID_SIZE]) {
	FILE *fp = fopen(filename, "r");
	int g_index= 0; 
	int j = 0;
	double d;
	if (fp == NULL) {
		printf("Can't open the file %s", filename);
		return;
	}
	while (!feof(fp) && g_index < CODE_SIZE) {
		for (j = 0; j < GRID_SIZE; j++) {
			fscanf(fp, "%lf,", &d);
			code_book[g_index][j] = d;
		}	
		g_index++;
	}
	fclose(fp);
}

void ReadHistDegree(const char *filename, float *hist_degree) {
	int index= 0; 
	int j = 0;
	int iv;
	float fv;

	FILE *fp = fopen(filename, "r");
	if (fp == NULL) {
		printf("Can't open the file %s", filename);
		return;
	}
	while (!feof(fp) && index < HISTOGRAM_SIZE) {
		fscanf(fp, "%d,%f", &iv, &fv);
		hist_degree[index++] = fv;
	}
	hist_degree[index] = FLOAT_MAX;
	fclose(fp);
}

void ReadBayesModel(const char *filename, BayesModel *bayes_model) {
	float fore_count = 0, back_count = 0;
	float tmp_fore, tmp_back;
	float rate_fore, rate_back;
	int index = 0;

	FILE *fp = fopen(filename, "r");
	if (fp == NULL) {
		printf("Can't open the file %s", filename);
		return;
	}
	fscanf(fp, "%f,%f", &fore_count, &back_count);
	while (!feof(fp) && index < BAYES_BIN_COUNT_MAX) {
		fscanf(fp, "%f,%f", &tmp_fore, &tmp_back);
		rate_fore = (tmp_fore + 1) / (float)(fore_count + 1); //plust one to smooth
        rate_back = (tmp_back + 1) / (float)(back_count + 1);
		bayes_model->fore_hist[index] = rate_fore;
		bayes_model->back_hist[index] = rate_back;
		index++;
	}
	bayes_model->fore_prob = fore_count / (float)(fore_count + back_count);
	bayes_model->back_prob = 1 - bayes_model->fore_prob;
	fclose(fp);
}

void ReadDictionary(char *filename, WeakLearner *weak_learner) {
	FILE *fp = fopen(filename, "r");
	int g_index= 0; 
	if (fp == NULL) {
		printf("Can't open the file %s", filename);
		return;
	}

	while (!feof(fp) && g_index < DICT_SIZE) {
		int index;
		float a, b, c;
		fscanf(fp, "%d,%f,%f,%f", &index, &a, &b, &c);
		weak_learner[g_index].index = index;
		weak_learner[g_index].threshold = a;
		weak_learner[g_index].conf1 = b;
		weak_learner[g_index].conf2 = c;
		g_index++;
	}
	fclose(fp);
}


/****************************************************************************************
  *******************                                          BackDiff Norm Sort                                  ********************
  ****************************************************************************************/
void InsertionSort(struct FloatCount *first, struct FloatCount *last) {
	struct FloatCount *i;
	if (first == last) return; 
	for (i = first + 1; i != last; ++i)  { //out loop
		//linear_insert(first, i);	// first,i is sub range => [first, i]
		struct FloatCount *last_tmp = i;
		struct FloatCount value = *last_tmp;		// record the last element
		struct FloatCount *result = last_tmp + 1;
		if (value.value < (*first).value) {	// the last element is smaller than the first element(then no comparision, just copy)
			//copy_backward(first, last, last + 1); // all the element move to the right one
			while (first != last_tmp) {
				*--result = *--last_tmp;
			}
			*first = value;		
		} else {
			//unguarded_linear_insert(last, value);
			struct FloatCount *next = last_tmp;
			--next;
			// the inner loop of insertion sort 
			// once no inversion, end the loop
			while (value.value < (*next).value) {	//inversion exists
				*last_tmp = *next;		
				last_tmp = next;			
				--next;				//swap the value
			}
			*last_tmp = value;
		}
	}
}


void CountSort(float *p, int length, int limit, float *min_v, float *max_v) {
	int i, j;
	struct FloatCount tmp_cnt[CLUSTER_SIZE];
	int min_index = limit;
    int max_index = length - limit;

	for (i = 0; i < CLUSTER_SIZE; i++) {
		tmp_cnt[i].value = -1;
		tmp_cnt[i].count = 0;
	}
	for (i = 0; i < length; i++) {
		for (j = 0; j < CLUSTER_SIZE; j++) {
			if (((tmp_cnt[j].value - (-1) < EPSILON) && ((-1) - tmp_cnt[j].value < EPSILON)) || ((p[i] - tmp_cnt[j].value < EPSILON) && (tmp_cnt[j].value - p[i] < EPSILON))) {
				tmp_cnt[j].value = p[i];
				tmp_cnt[j].count++;
				break;
			}
		}
	}
	InsertionSort(tmp_cnt, tmp_cnt + CLUSTER_SIZE);
	for (i = 1; i < CLUSTER_SIZE; i++) {
		tmp_cnt[i].count += tmp_cnt[i - 1].count;
	}
	for (i = 0; i < CLUSTER_SIZE; i++) {
		if (min_index < tmp_cnt[i].count) {
			*min_v = tmp_cnt[i].value;
			break;
		}
	}
	for (i = CLUSTER_SIZE - 1; i > 0; i--) {
		if (max_index < tmp_cnt[i].count && max_index > tmp_cnt[i - 1].count) {
			*max_v = tmp_cnt[i].value;
			break;
		}
	}
}

/****************************************************************************************
  *******************                                          ShortPath heap Sort                                ********************
  ****************************************************************************************/
int MinHeapCom(float a, float b) {
	return a < b;
}
int MaxHeapCom(float a, float b) {
	return a > b;
}
void AdjustHeap(float *p, int hole_index, int length, HeapCompare cmp) {
	while (hole_index < length) {
		int lchild = 2 * hole_index + 1;
		int rchild = 2 * hole_index + 2;
        int small_num = lchild;
		if (lchild < length && cmp(p[lchild], p[hole_index])) {
			small_num = lchild;
		} 
        else {
			small_num = hole_index;
		}
		if (rchild < length && cmp(p[rchild], p[small_num])) {
			small_num = rchild;
		}
		if (small_num != hole_index) {
			float tmp = p[hole_index];
			p[hole_index] = p[small_num];
			p[small_num] = tmp;
			hole_index = small_num;
		} 
        else {
			break;
		}
	}
}
void MakeHeap(float *p, int length, HeapCompare cmp) {
	int i;
	for (i = (length) / 2; i >= 0; --i) {
		AdjustHeap(p, i, length, cmp);
	}
}

void SortHeap(float *p, int length, HeapCompare cmp) {
	MakeHeap(p, length, cmp);
	while (length-- > 0) {
		float tmp = p[0];
		p[0] = p[length];
		p[length] = tmp;
		AdjustHeap(p, 0, length, cmp);
	}
}
float FindMaxOrMin(float *p, int length, HeapCompare cmp) {
	float result = p[0];
	int i;
	for (i = 1; i < length; i++) {
		if (cmp(result, p[i])) {
			result = p[i];
		}
	}
	return result;
}


void GetImageFloat(float *index_float, unsigned char *cluster_map, float *img_float, int width, int height) {
	int x, y;
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			IMAGE_ELEM(img_float, float, width, x, y) = index_float[IMAGE_ELEM(cluster_map, unsigned char, width, x, y)];
		}
	}
	//SaveFloatImage("yon", img_float, width, height);
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			IMAGE_ELEM(img_float, float, width, x, y) = IMAGE_ELEM(img_float, float, width, x, y)  > 0? 255 :0 ;
		}
	}
}
/******************************************************************************************
 **************************         write float image            **************************
 ******************************************************************************************/
void FloatToImage(float *img_float, int width, int height, unsigned char *img_data) {
	int x, y;
	int img_width_step = CALCULATE_STEP_THREE(width);
	int res_width_step = width;
	float max_v = FLOAT_MIN, min_v = FLOAT_MAX;

	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			float pixel_value = IMAGE_ELEM(img_float, float, res_width_step, x, y);
			if (pixel_value < 0) {
				pixel_value = 0;
			}
			if (max_v < pixel_value) {
				max_v = pixel_value;
			}
			if (min_v > pixel_value) {
				min_v = pixel_value;
			}
		}
	}
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			float pixel_value = IMAGE_ELEM(img_float, float, res_width_step, x, y);
			int value = (int)((pixel_value - min_v) * 255 / (max_v - min_v) + 0.5);
			if (value < 0) {
				value = 0;
			}
			if (value > 255) {
				value = 255;
			}
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 0, y) = value;
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 1, y) = value;
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 2, y) = value;
		}
	}
}


void SaveFloatImage(char *suffix, float *img_float, int width, int height) {
	char file_output[MAX_LENGTH] = {0};
	unsigned char img_data[THREE_CHANNEL_IMAGE_MAX_PIXEL] = {0};
	strncat(file_output, g_output_filename, strrchr(g_output_filename, '_') - g_output_filename + 1); 
	strcat(file_output, suffix);
	strcat(file_output, ".bmp");
	//printf("%s\n", file_output);
	FloatToImage(img_float, width, height, img_data);
	//WriteImage(file_output, img_data, width, height);
}

void SaveYonImage(float *index_float, unsigned char *cluster_map, int width, int height) {
	float img_float[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0};
	int x, y;
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			IMAGE_ELEM(img_float, float, width, x, y) = index_float[IMAGE_ELEM(cluster_map, unsigned char, width, x, y)];
		}
	}
	SaveFloatImage("yon", img_float, width, height);
}
void SaveShortpathImage(float *img_float, int width, int height) {
	SaveFloatImage("shortpath", img_float, width, height);
}
void SaveBackdiffImage(float *index_float, unsigned char *cluster_map, int width, int height) {
	float img_float[ONE_CHANNEL_IMAGE_MAX_PIXEL] = {0.0};
	int x, y;
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			IMAGE_ELEM(img_float, float, width, x, y) = index_float[IMAGE_ELEM(cluster_map, unsigned char, width, x, y)];
		}
	}
	SaveFloatImage("backdiff", img_float, width, height);
}
void SaveUcharImage(char *suffix, unsigned char *img, int width, int height) {
	char file_output[MAX_LENGTH] = {0};
	unsigned char img_data[THREE_CHANNEL_IMAGE_MAX_PIXEL] = {0};
	int x,y;
	unsigned char pixel_value;
	int img_width_step = 3* width;
	for (y = 0; y < height; y++) {
		for (x = 0; x < width; x++) {
			pixel_value = IMAGE_ELEM(img, unsigned char, width, x, y);
			
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 0, y) = pixel_value;
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 1, y) = pixel_value;
			IMAGE_ELEM(img_data, unsigned char, img_width_step, x * 3 + 2, y) = pixel_value;
		}
	}

	strncat(file_output, g_output_filename, strrchr(g_output_filename, '_') - g_output_filename + 1); 
	strcat(file_output, suffix);
	strcat(file_output, ".bmp");
	//WriteImage(file_output, img_data, width, height);
}

void SaveAFImage(unsigned char *img, int width, int height) {
	SaveUcharImage("AF", img, width, height);
}
void SaveRoughImage(unsigned char *img, int width, int height) {
	SaveUcharImage("rough", img, width, height);
}